#include "movecontrol.h"

volatile int speed_FL = g_baseSpeed;
volatile int speed_FR = g_baseSpeed;
volatile int speed_RL = g_baseSpeed;
volatile int speed_RR = g_baseSpeed;

// 分配PWM通道（ESP32最多16个通道）
const int channels[8] = {
    0,  // FL_A
    1,  // FL_B
    2,  // FR_A
    3,  // FR_B
    4,  // RL_A
    5,  // RL_B
    6,  // RR_A
    7   // RR_B
  };

void freeSpeed(){
    Serial.printf("after fresh speed is %d\n",g_baseSpeed);
    speed_FL = g_baseSpeed / 1.02;
    speed_FR = g_baseSpeed / 0.94;
    speed_RL = g_baseSpeed;
    speed_RR = g_baseSpeed * 1.10;
}

void setupPWM() {
    // 初始化所有通道
    for(int ch=0; ch<8; ch++){
        ledcSetup(ch, PWM_FREQ, PWM_RES);
    }
    // 前左轮
    ledcAttachPin(MOTOR_FL_A, channels[0]);
    ledcAttachPin(MOTOR_FL_B, channels[1]);

    // 前右轮
    ledcAttachPin(MOTOR_FR_A, channels[2]);
    ledcAttachPin(MOTOR_FR_B, channels[3]);

    // 后左轮
    ledcAttachPin(MOTOR_RL_A, channels[4]);
    ledcAttachPin(MOTOR_RL_B, channels[5]);

    // 后右轮
    ledcAttachPin(MOTOR_RR_A, channels[6]);
    ledcAttachPin(MOTOR_RR_B, channels[7]);
}

void motorControl(uint8_t pinA, uint8_t pinB, int speed) {
    speed = constrain(speed, 0, 255);
    Serial.printf("motor speed is %d\n",speed);
    ledcWrite(pinA, speed);
    ledcWrite(pinB, 0);
}

// 麦克纳姆轮运动控制函数
void moveForward() {
    motorControl(channels[0], channels[1], speed_FL);
    motorControl(channels[2], channels[3], speed_FR);
    motorControl(channels[4], channels[5], speed_RL);
    motorControl(channels[6], channels[7], speed_RR);
}

void moveBackward() {
    motorControl(channels[1], channels[0], speed_FL);
    motorControl(channels[3], channels[2], speed_FR);
    motorControl(channels[5], channels[4], speed_RL);
    motorControl(channels[7], channels[6], speed_RR);
}

void moveLeft() {
    motorControl(channels[1], channels[0], speed_FL);
    motorControl(channels[2], channels[3], speed_FR);
    motorControl(channels[4], channels[5], speed_RL);
    motorControl(channels[7], channels[6], speed_RR);
}

void moveRight() {
    motorControl(channels[0], channels[1], speed_FL);
    motorControl(channels[3], channels[2], speed_FR);
    motorControl(channels[5], channels[4], speed_RL);
    motorControl(channels[6], channels[7], speed_RR);
}

void stopMotors() {
    motorControl(channels[0], channels[1], 0);
    motorControl(channels[2], channels[3], 0);
    motorControl(channels[4], channels[5], 0);
    motorControl(channels[6], channels[7], 0);
}

// 左后方移动
void moveBackwardLeft() {
    motorControl(channels[1], channels[0], speed_FL);
    motorControl(channels[2], channels[3], 0);
    motorControl(channels[4], channels[5], 0);
    motorControl(channels[7], channels[6], speed_RR);
}

// 左前方移动
void moveForwardLeft() {
    motorControl(channels[0], channels[1], 0);
    motorControl(channels[2], channels[3], speed_FR);
    motorControl(channels[4], channels[5], speed_RL);
    motorControl(channels[6], channels[7], 0);
}

// 右前方移动
void moveForwardRight() {
    motorControl(channels[0], channels[1], speed_FL);
    motorControl(channels[2], channels[3], 0);
    motorControl(channels[4], channels[5], 0);
    motorControl(channels[6], channels[7], speed_RR);
}

// 右后方移动
void moveBackwardRight() {
    motorControl(channels[1], channels[0], 0);
    motorControl(channels[3], channels[2], speed_FR);
    motorControl(channels[5], channels[4], speed_RL);
    motorControl(channels[7], channels[6], 0);
}

// 顺时针选择
void clockwise() {
    motorControl(channels[0], channels[1], speed_FL);
    motorControl(channels[3], channels[2], speed_FR);
    motorControl(channels[4], channels[5], speed_RL);
    motorControl(channels[7], channels[6], speed_RR);
}

// 逆时针选择
void antiClockwise() {
    motorControl(channels[1], channels[0], speed_FL);
    motorControl(channels[2], channels[3], speed_FR);
    motorControl(channels[5], channels[4], speed_RL);
    motorControl(channels[6], channels[7], speed_RR);
}
